# %%
import math

import astropy.units as u
import numpy as np
from astropy.constants import R_earth
from astropy.coordinates import Angle
from astropy.time import Time

from mission_parameters import JD_FR_START, JD_START

SUN_ANGLE = Angle("0:32:0 degrees").radian / 2
# same as Angle("0°32′00″") but without confusing symbols
# average between 31′27″ – 32′32″
# assume it constant and the same size from any point of satellite orbit

SIM_START_TIME = Time(JD_START, JD_FR_START, format="jd").value
# this calculation is CPU consuming, thus make it once and global


# %%
def sun_vector(current_time):
    """Sun unit vector in ECI

    Parameters
    ----------
    current_time : float
        current simulation time started from zero

    Returns
    -------
    r_earth_sun : 1x3 array
        sun position in ECI
    """
    # page 420 in book
    # Fundamentals of spacecraft attitude determination and control

    # CPU consuming with astropy library usage:
    # sim_start_time = Time(JD_START, JD_FR_START, format="jd")
    # JD = sim_start_time + current_time * u.second
    # JD = JD.value

    # Fast method, not using astropy library:
    JD = SIM_START_TIME + current_time / 86400

    t_ut1 = (JD - 2451545) / 36525

    phi_sun = math.radians(280.460 + 36000.771 * t_ut1)  # mean longitude
    phi_sun = phi_sun % (2 * math.pi)

    M_sun = math.radians(357.5277233 + 35999.05034 * t_ut1)  # mean anomaly
    M_sun = M_sun % (2 * math.pi)

    phi_ecliptic = (
        phi_sun
        + math.radians(1.914666471) * math.sin(M_sun)
        + 0.019994643 * math.sin(2 * M_sun)
    )  # longitude of the ecliptic
    eps_ecliptic = math.radians(
        23.439291 - 0.0130042 * t_ut1
    )  # obliquity of the ecliptic

    # r_earth_sun = get_sun(t).cartesian.xyz.to(u.m).value
    e_earth_sun = [
        math.cos(phi_ecliptic),
        math.cos(eps_ecliptic) * math.sin(phi_ecliptic),
        math.sin(eps_ecliptic) * math.sin(phi_ecliptic),
    ]

    return e_earth_sun


# %%
def sun_visibility(current_time, r_earth_sat):
    """Defines Sun visibility from satellite position.

    Parameters
    ----------
    current_time : float
        simulation time starting from zero, second.
    r_earth_sat - 1x3 numpy array
        vector of satellite coordinates in ECI, m.

    Returns
    -------
    float
        1. is for fully visible
        0. is for full eclipse
    """
    # https://en.wikipedia.org/wiki/Angular_diameter

    # r_earth_sun = sun_vector(current_time)
    # Position vector from the spacecraft to the Sun, GCI frame
    # r_sat_sun = r_earth_sun - r_earth_sat
    # e_sat_sun = r_sat_sun / np.linalg.norm(r_sat_sun)
    e_sat_sun = sun_vector(current_time)

    # calculations are made in satellite centered frame with axis parallel to ECI
    # TODO convert calculations to orbital frame, should be the same result
    r_earth_sat = np.array(r_earth_sat)
    r_sat_earth = -r_earth_sat
    e_sat_earth = r_sat_earth / np.linalg.norm(r_sat_earth)

    earth_angle = math.asin(R_earth.to(u.m).value / np.linalg.norm(r_earth_sat))
    s = math.acos(np.dot(e_sat_earth, e_sat_sun))

    # Calculate the size of the lune (http://mathworld.wolfram.com/Lune.html) in rad
    # Consider calculation in a plane on distance 1 meter from satellite
    # so we can can compare angels like radiuses
    if s < (earth_angle + SUN_ANGLE) and s > (earth_angle - SUN_ANGLE):
        lunedelta = 0.25 * math.sqrt(
            (SUN_ANGLE + earth_angle + s)
            * (earth_angle + s - SUN_ANGLE)
            * (s + SUN_ANGLE - earth_angle)
            * (SUN_ANGLE + earth_angle - s)
        )
        lune_area = (
            2 * lunedelta
            + SUN_ANGLE
            * SUN_ANGLE
            * (
                math.acos(
                    ((earth_angle * earth_angle) - (SUN_ANGLE * SUN_ANGLE) - (s * s))
                    / (2 * SUN_ANGLE * s)
                )
            )
            - earth_angle
            * earth_angle
            * (
                math.acos(
                    ((earth_angle * earth_angle) + (s * s) - (SUN_ANGLE * SUN_ANGLE))
                    / (2 * earth_angle * s)
                )
            )
        )
        res = lune_area / (math.pi * SUN_ANGLE * SUN_ANGLE)

    elif s <= (earth_angle - SUN_ANGLE):
        res = 0
    else:  # If s>earth_angle+SUN_ANGLE there is no eclipse taking place
        res = 1

    return res


# %%
def sun_sensor_measure(
    sun_vector_body: np.ndarray, sun_sensor_matrix: np.ndarray, sun_visibility: float
) -> np.ndarray | None:
    """Translation of the unit direction vector on the Sun from the satellite
    coordinate system to the solar sensor coordinate system.

    Parameters
    ----------
    sun_vector_body : np.ndarray
        Unit position vector of the Sun in the satellite coordinate system, m
    sun_sensor_matrix : np.ndarray
        Solar Sensor Installation Matrix
    sun_visibility : float
        Visibility of the Sun (0 - the Sun is not visible, 1 - fully visible,
        0..1 - the Sun is partially visible)

    Returns
    -------
    np.ndarray|None
        np.ndarray. If sun_visibility >= 0.5
        None. If sun_visibility < 0.5
    """
    sun_vector_sensor = None

    if sun_visibility >= 0.5:
        sun_vector_sensor = np.dot(sun_sensor_matrix.T, sun_vector_body)

    return sun_vector_sensor
